Double Pendulum simulation

Equations of Motion - Hand Derived

Kinematic Constraints

, ,

, ,

Potential Energy

Kinetic Energy

Euler-Lagrange Equations

Lagrangian:

Euler-Lagrange equations:

where represents the external (non-conservative) forces applied

Equations of Motion - MATLAB Derived

Kinematic Constraints

, ,

, ,

syms q(t) l1 theta_1(t) l2 theta_2(t)

xq = q;

x1 = xq + l1*sin(theta_1);

x2 = x1 + l2*sin(theta_2);

yq = 0;

y1 = yq + l1*cos(theta_1);

y2 = y1 + l2*cos(theta_2);

 

% derivatives

xq_dot = diff(q,t)

xq_dot(t) = 

x1_dot = diff(x1,t)

x1_dot(t) = 

x2_dot = diff(x2,t)

x2_dot(t) = 

yq_dot = 0

yq_dot = 0

y1_dot = diff(y1,t)

y1_dot(t) = 

y2_dot = diff(y2,t)

y2_dot(t) = 

Potential Energy

syms m_q m1 m2 g

V = m_q*g*yq + m1*g*y1 + m2*g*y2

V(t) = 

Kinetic Energy

K = 1/2*(m_q*(xq_dot^2 + yq_dot^2) + m1*(x1_dot^2 + y1_dot^2) + m2*(x2_dot^2 + y2_dot^2))

K(t) = 

K =

Euler-Lagrange Equations

Lagrangian:

Euler-Lagrange equations:

where represents the external (non-conservative) forces applied

L = simplify(expand(K - V))

L(t) = 

Q0; q

dLdq_dot = collect(simplify(expand(diff(L,diff(q,t)))),[diff(q,t) diff(theta_1,t) diff(theta_2,t)]);

ddt_dLdq_dot = diff(dLdq_dot,t);

dLdq = diff(L,q);

Q0 = collect(simplify(expand(ddt_dLdq_dot - dLdq)),[diff(q,t,2), diff(theta_1,t,2), diff(theta_1,t), diff(theta_2,t,2), diff(theta_2,t)])

Q0(t) = 

Q1:

dLdtheta_1_dot = collect(simplify(expand(diff(L,diff(theta_1,t)))),[diff(q,t) diff(theta_1,t) diff(theta_2,t)]);

ddt_dLdtheta_1_dot = diff(dLdtheta_1_dot,t);

dLdtheta_1 = diff(L,theta_1);

Q1 = collect(simplify(expand(ddt_dLdtheta_1_dot - dLdtheta_1)),[diff(q,t,2), diff(theta_1,t,2), diff(theta_1,t), diff(theta_2,t,2), diff(theta_2,t)])

Q1(t) = 

Q2:

dLdtheta_2_dot = collect(simplify(expand(diff(L,diff(theta_2,t)))),[diff(q,t) diff(theta_1,t) diff(theta_2,t)]);

ddt_dLdtheta_2_dot = diff(dLdtheta_2_dot,t);

dLdtheta_2 = diff(L,theta_2);

Q2 = collect(simplify(expand(ddt_dLdtheta_2_dot - dLdtheta_2)),[diff(q,t,2), diff(theta_1,t,2), diff(theta_1,t), diff(theta_2,t,2), diff(theta_2,t)])

Q2(t) = 

Euler-Lagrange with Non-Conservative Forces

assume there is a disturbance, w and a frictional damping force at each node; the cart, m1 and m2

Setting up the State-Space equations

let and

y_dot = [diff(q,t); diff(theta_1,t); diff(theta_2,t)];

y_ddot = [sol.q_ddot; sol.theta_ddot_1; sol.theta_ddot_2];

syms x0 x1 x2 x3 x4 x5

x_dot = subs(simplify(expand([y_dot; y_ddot])),[diff(q,t), diff(theta_1,t), diff(theta_2,t), q, theta_1, theta_2], [x3, x4, x5, x0, x1, x2]);

x_dot = x_dot(t)

x_dot = 

symvar(x_dot)

ans = 

Linearize

x_dot_internal = subs(x_dot,[w0, w1, w2], [0 0 0]);

x = [x0; x1; x2; x3; x4; x5];

Asym = jacobian(x_dot_internal,x)

Asym = 

Bsym = jacobian(x_dot_internal,u)

Bsym = 

Setting equilibrium point to [0; 0; 0; 0; 0; 0]

A0 = simplify(subs(Asym,x,zeros(6,1)))

A0 = 

B0 = simplify(subs(Bsym,x,zeros(6,1)))

B0 = 

Subbing in Some real values to aid in simulation

vars = symvar(A0+B0)

vars = 

% [1/s] friction damping coefficient

d0_real = 0.05;

d1_real = 0.05;

d2_real = 0.05;

g_real = 9.8; % [m/s^2]

l1_real = 1e-1; % [m]

k = 1; % fraction of l2/l1

l2_real = k*l1; % [m]

m1_real = 1; % [kg]

m2_real = 1; % [kg]

m_q_real = 1; % [kg]

vars_real = [d0_real, d1_real, d2_real, g_real, l1_real, l2_real, m1_real, m2_real, m_q_real];

A0_real = subs(A0,vars,vars_real)

A0_real = 

A0_real = double(A0_real);

B0_real = subs(B0,vars,vars_real)

B0_real = 

B0_real = double(B0_real);

C = [eye(3) zeros(3,3)]; D = zeros(3,1);

rank(ctrb(A0_real,B0_real))

ans = 6

rank(obsv(A0_real,C))

ans = 6

olsys = ss(A0_real,B0_real,C,D);

s = tf('s');

oltf = C*inv(s*eye(6) - A0_real)*B0_real;

rlocus(olsys(1))

bode(olsys(1))

rlocus(olsys(2))

bode(olsys(2))

rlocus(olsys(3))

bode(olsys(3))

Stabilizing the System with Proportional Gain Feedback

% placing poles with 4 poles optimally damped and 1 set of poles critically damped

% speed of the poles chosen to balance damping with speed

p = [-5, -5, -10*(1 + 1i*[1, -1]), -10.6*(1 + 1i*[1, -1])];

gains = acker(A0_real,B0_real,p)

gains = 1×6

58.4965 -197.6694 286.9746 39.1935 0.8044 14.8938

% u = -Kx + r => xdot = (A - B*K)*x + B*r

Abar = A0_real-B0_real*gains;

% Closed Loop system

states = {'x' 'theta_1' 'theta_1' 'x_dot' 'theta_dot_1' 'theta_dot_2'};

inputs = {'u'};

outputs = {'x'; 'theta_1'; 'theta_2'};

clsys = ss(Abar,B0_real,C,D,'statename',states,'inputname',inputs,'outputname',outputs);

rlocus(clsys(1))

margin(clsys(1))

rlocus(clsys(2))

margin(clsys(2))

rlocus(clsys(3))

margin(clsys(3))

step(clsys)

initial(clsys,[0.05; -10*pi/180; -2*pi/180; 0; 0; 0])

Iterating Through Ratios of Length to discover patterns

% [1/s] friction damping coefficient

k = [0.1, 0.5, 0.75, 1, 5, 10, 50, 100]; % logspace(-2,3,10);

l2_iter = k*l1; % [m]

vars_iter = zeros(size(vars,1), size(vars,2),length(k));

A0_iter = zeros(6,6,length(k));

B0_iter = zeros(6,1,length(k));

gains = zeros(1,6,length(k));

Abar = zeros(size(A0_iter));

t = 0:0.01:4;

states = {'x' 'theta_1' 'theta_1' 'x_dot' 'theta_dot_1' 'theta_dot_2'};

inputs = {'u'};

outputs = {'x'; 'theta_1'; 'theta_2'};

clear clsys_iter

for i = 1:length(k)

vars_iter(:,:,i) = [d0_real, d1_real, d2_real, g_real, l1_real, l2_iter(i), m1_real, m2_real, m_q_real];

A0_iter(:,:,i) = double(subs(A0,vars,vars_iter(:,:,i)));

B0_iter(:,:,i) = double(subs(B0,vars,vars_iter(:,:,i)));

% placing poles at the same locations... will see if that has an effect

gains(:,:,i) = acker(A0_iter(:,:,i),B0_iter(:,:,i),p);

% u = -Kx + r => xdot = (A - B*K)*x + B*r

Abar(:,:,i) = A0_iter(:,:,i)-B0_iter(:,:,i)*gains(:,:,i);

% Closed Loop system

clsys_iter(:,:,i) = ss(Abar(:,:,i),B0_iter(:,:,i),C,D,'statename',states,'inputname',inputs,'outputname',outputs);

[y(:,:,i), ~, x_iter(:,:,i)] = initial(clsys_iter(:,:,i),[0.05; -10*pi/180; -2*pi/180; 0; 0; 0],t);

end

 

%%

 

figure(20);

tiledlayout(3,1);

ax1 = nexttile; hold on; grid on;

ax1.ColorOrder = [1 0 0; 0 0 1; 0 0.7 0; 0 0 0; 0.929 0.694 0.125];

ax1.LineStyleOrder = {'-','--'};

ax2 = nexttile; hold on; grid on;

ax2.ColorOrder = [1 0 0; 0 0 1; 0 0.7 0; 0 0 0; 0.929 0.694 0.125];

ax2.LineStyleOrder = {'-','--'};

ax3 = nexttile; hold on; grid on;

ax3.ColorOrder = [1 0 0; 0 0 1; 0 0.7 0; 0 0 0; 0.929 0.694 0.125];

ax3.LineStyleOrder = {'-','--'};

title(ax1,'Response to Initial Conditions');

ylabel(ax1,'To: x [m]');

ylabel(ax2,'To: \theta_1 [deg]');

ylabel(ax3,'To: \theta_2 [deg]'); xlabel('Time (seconds)');

for i = 1:length(k)

plot(ax1,t, y(:,1,i));

plot(ax2,t,y(:,2,i)*180/pi);

 

plot(ax3,t,y(:,3,i)*180/pi);

 

end

maxmin = 5;

degmax = 30;

ylim(ax1,[-1,1]*maxmin)

ylim(ax2,[-1 1]*degmax)

ylim(ax3,[-1 1]*degmax)

axes(ax1)

legend(num2str(k'))

Conclusion

[0.05; -10*pi/180; -2*pi/180; 0; 0; 0] =